/*
该任务用来处理来自不同控制方式下传回的期望的控制指令在此处统一为通用格式的控制指令发送给其他任务

*/



/* Includes ----------------------------------------------------------------- */
#include "cmd.h"
#include "gpio.h"

#include <string.h>

/* Private function  -------------------------------------------------------- */
/*Export function --------------------------------------------------------------*/
int8_t CMD_Init(CMD_t *cmd){
   /*若主结构体为空 自动返回错误 */
  if(cmd == NULL) return-1;
 /**/
  cmd->C_cmd.type =RC;
	cmd->C_cmd.mode =NORMAL;

return 0;
}


static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd) {

  /* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */
	
  cmd->Vx = rc->ch_r_x;  
  cmd->Vy = -rc->ch_r_y;  //此处为了适应全向轮加了负号
  cmd->Vw = -rc->ch_l_x;

  cmd->poscamear = rc->ch_l_y;

  cmd->key_ctrl = rc->sw_l;
  
}



/**
 * @brief rc失控时机器人恢复放松模式
 *
 * @param cmd 主结构体
 */
static void CMD_RcLostLogic(CMD_t *cmd){
  /* 机器人底盘运行模式恢复至放松模式 */
  cmd->C_cmd.mode = RELAXED;
}
int8_t CMD_ParseRc(CMD_t *cmd,CMD_RC_t *rc){   
   if (cmd == NULL) return -1;
   if (rc == NULL) return -1;
  /*c当rc丢控时，恢复机器人至默认状态 */
  if ((rc->sw_l == CMD_SW_ERR) || (rc->sw_r == CMD_SW_ERR)) {
    CMD_RcLostLogic(cmd);
  } else {
      CMD_RcLogic(rc, cmd);
  }
return 0;
}



int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n){
  if(cmd == NULL) return -1;
  if(n == NULL) return -1;

  cmd->cmd_status = n->status_fromnuc;
  cmd->raw_status = n->ctrl_status;
	
	 for (int i = 0; i < 7; ++i) 
	  { // 从最低位到最高位遍历
        cmd->status[i] = ((cmd->raw_status) & (1 << i)) ? 1 : 0;
    }
    switch(cmd->cmd_status){
			case PICK:
			cmd->C_pick.posx= n->pick.posx;
			cmd->C_pick.posy= n->pick.posy;
			cmd->C_pick.posw= n->pick.posw;
		    break;
			case MID:
			cmd->C_navi.vx = n->navi.vx;
			cmd->C_navi.vy = n->navi.vy;
			cmd->C_navi.wz = n->navi.wz;
			  break;
			case SICK_CAIL:
			cmd->forsick_wz = n->sick_cali.angle;
			  break;
		}
 if(cmd->cmd_status !=PICK &&cmd->cmd_status !=MID &&cmd->cmd_status !=SICK_CAIL)
 {
	 cmd->cmd_status = RCTRL;
 }
return 0;
}

int8_t CMD_CtrlSet(CMD_t *cmd) {

 if(cmd == NULL) return -1;
  if(cmd->key_ctrl == CMD_SW_UP)//当拨杆打到最上面时 强制使用遥控器控制
	{
		cmd->C_cmd.type = RC;
		cmd->C_cmd.mode = NORMAL;
	}
	else if(cmd->key_ctrl ==CMD_SW_DOWN)
	{
		 cmd->C_cmd.type = RC;
		 cmd->C_cmd.mode = PUT;
	}
	else 
	{		
  switch(cmd->cmd_status){
	 case PICK:
		 cmd->C_cmd.type = CAMERA_PICK;
//	   cmd->C_cmd.mode = NORMAL;
	   break;
	 case MID:
		 cmd->C_cmd.type = MID_NAVI; 
	   break;
	 case SICK_CAIL:
		  cmd->C_cmd.type = SICK_CAILING;
	   break;
	 case RCTRL:
		 cmd->C_cmd.type = RC;
	   cmd->C_cmd.mode = NORMAL;
	 break;
 }
 
//  switch(cmd->status[4]){
//		case 0:
//		 cmd->C_cmd.mode =GYRO_STAY;
//		break;
//		case 1:
//		 cmd->C_cmd.mode=NORMAL;
//		break;
//	}
  }
return 0;
}


